#include "modules/drivers/lidar/orin_hesai/parser/lidar_parser.h"

#include "cyber/cyber.h"

namespace apollo {
namespace drivers {
namespace lidar_hesai {
LidarParser::LidarParser() {}

bool LidarParser::Init(
    std::shared_ptr<CircularQueueBuffer<LidarPacket>> packet_buffer,
    std::shared_ptr<CircularQueueBuffer<PointCloud>> pc_buffer) {
  packet_buffer_ = packet_buffer;
  pc_buffer_ = pc_buffer;
  seq_ = 0;
  drop_count_ = 0;
  frame_interval_ = FLAGS_frame_interval;
  frame_last_azimuth_ = 40000;
  frame_out_ = std::make_shared<PointCloud>();
  cloud_buf_ = new PointCloud;
  auto header_buf = frame_out_->mutable_header();
  header_buf->set_radar_timestamp(0);  // used as startStamp
  header_buf->set_lidar_timestamp(0);  // used as endStamp
  top_lidar_packet_num_ = 0;

  frame_azimuth_.resize(PACKET_PER_FRAME, 0);

  if (cloud_buf_ == nullptr) {
    AERROR << "PointCloud buffer init failed!";
    return false;
  }
  cloud_buf_->Clear();
  if (!hesai_parser_.Init()) {
    AERROR << "Hesai parser init failed!";
    return false;
  }
  return true;
}

void LidarParser::Start() {
  AWARN << "[thread_name]lidar_parser";
  while (!cyber::IsShutdown()) {
    if (!packet_buffer_->Get(&packet_)) {
      std::this_thread::sleep_for(std::chrono::nanoseconds(1000000));
      continue;
    }
    // is first frame?
    if (frame_out_->header().radar_timestamp() == 0) {
      uint64_t sys_time =
          std::chrono::high_resolution_clock::now().time_since_epoch().count();
      uint64_t pkt_time = packet_.stamp;
      if (sys_time - pkt_time > 40'000'000 &&
          pkt_time - sys_time > 40'000'000) {
        AERROR << "packet check error: SysTime " << sys_time
               << " PointCloudTime " << pkt_time;
        continue;
      }
      frame_out_->mutable_header()->set_radar_timestamp(
          packet_.stamp - packet_.stamp % frame_interval_ + frame_interval_);
      frame_out_->mutable_header()->set_lidar_timestamp(
          frame_out_->header().radar_timestamp() + frame_interval_);
      frame_out_->mutable_header()->set_sequence_num(seq_);

      AINFO << "first frame, start stamp: "
            << frame_out_->header().radar_timestamp()
            << ", end stamp: " << frame_out_->header().lidar_timestamp();
    }

    // skip too early packets
    if (packet_.stamp < frame_out_->header().radar_timestamp()) {
      ++drop_count_;

      continue;
    }
    // send old and create new frame when receive new packet
    if (packet_.stamp > frame_out_->header().lidar_timestamp() ||
        top_lidar_packet_num_ == PACKET_PER_FRAME) {
      InsertFrame();
    }
    // parse data for current frame
    HesaiDataProcess(&packet_);
    top_lidar_packet_num_++;
  }
}

void LidarParser::InsertFrame() {
  AINFO << "InsertFrame " << start_azimuth_ << " " << end_azimuth_
        << " pkt_num: " << top_lidar_packet_num_
        << " drop_count_: " << drop_count_;
  drop_count_ = 0;
  if (frame_out_->point_size() < POINT_PER_FRAME) {
    uint32_t missedPoint = POINT_PER_FRAME - frame_out_->point_size();
    AERROR << "Frame end: top lidar " << missedPoint
           << " points missed, append zeros.";
    HesaiParser::AppendZeroPoint(missedPoint, frame_out_.get());
  }

  if (!pc_buffer_->Insert(frame_out_.get())) {
    AINFO << "Frame buffer full! Discard oldest frame.";
    return;
  }

  seq_++;
  uint64_t lastStamp = frame_out_->header().lidar_timestamp();
  while (lastStamp + frame_interval_ < packet_.stamp) {
    lastStamp += frame_interval_;
  }
  frame_out_->Clear();
  frame_last_azimuth_ = 40000;
  frame_out_->mutable_header()->set_radar_timestamp(lastStamp);
  frame_out_->mutable_header()->set_lidar_timestamp(lastStamp +
                                                    frame_interval_);
  frame_out_->mutable_header()->set_sequence_num(seq_);
  top_lidar_packet_num_ = 0;

  frame_azimuth_.resize(PACKET_PER_FRAME, 0);
}

void LidarParser::HesaiDataProcess(const LidarPacket* packet) {
  cloud_buf_->Clear();
  bool ret = hesai_parser_.ParseData(&(packet->hesaiPacket), cloud_buf_,
                                     start_azimuth_, end_azimuth_);
  if (!ret) return;
  if (frame_last_azimuth_ > 36000) {
    // current_frame_->startAzimuth = start_azimuth_;
    frame_last_azimuth_ = 520;
    AINFO << "hesai start_azimuth: " << start_azimuth_;
  }
  frame_out_->MergeFrom(*cloud_buf_);
}

}  // namespace lidar_hesai
}  // namespace drivers
}  // namespace apollo